/*
 * @Author: Jiagang Chen
 * @Date: 2021-11-11 13:52:35
 * @LastEditors: Jiagang Chen
 * @LastEditTime: 2021-12-01 08:36:36
 * @Description: ...
 * @Reference: ...
 */

#include "kaist_publisher/velodyne_converter.h"

namespace kaist_publisher {

float scanPeriod = 0.1;

static bool comp(std::pair<int64_t, std::string> &a,
                 std::pair<int64_t, std::string> &b) {
    return a.first < b.first;
}

VelodyneConverter::VelodyneConverter(ros::NodeHandle &nh,
                                     const std::string &dataset_dir,
                                     const std::string &save_dir,
                                     const std::string &left_topic,
                                     const std::string &right_topic)
    : Converter(dataset_dir, save_dir),
      left_topic_(left_topic),
      right_topic_(right_topic) {
    left_bag_name_ = FilterSlash(left_topic_) + ".bag";
    right_bag_name_ = FilterSlash(right_topic_) + ".bag";
    pub_velodyne_left_ =
        nh.advertise<sensor_msgs::PointCloud2>(left_topic_.c_str(), 2);
    ;
    pub_velodyne_right_ =
        nh.advertise<sensor_msgs::PointCloud2>(right_topic_.c_str(), 2);
    ;
}

int64_t VelodyneConverter::Init_Publish() {
    const std::string stamp_file_left =
        dataset_dir_ + "/" + default_left_stamp_file;
    ;
    FILE *fp_left = fopen(stamp_file_left.c_str(), "r");
    int64_t stamp_left;
    while (fscanf(fp_left, "%ld\n", &stamp_left) == 1) {
        all_stamps_.push_back(
            std::pair<int64_t, std::string>(stamp_left, "left"));
    }
    fclose(fp_left);

    const std::string stamp_file_right =
        dataset_dir_ + "/" + default_right_stamp_file;
    ;
    FILE *fp_right = fopen(stamp_file_right.c_str(), "r");
    int64_t stamp_right;
    while (fscanf(fp_right, "%ld\n", &stamp_right) == 1) {
        all_stamps_.push_back(
            std::pair<int64_t, std::string>(stamp_right, "right"));
    }
    fclose(fp_right);

    if (all_stamps_.size() >= 1) {
        std::sort(all_stamps_.begin(), all_stamps_.end(), &comp);
        return all_stamps_[idx_].first;
    } else {
        return LONG_MAX;
    }
}

int64_t VelodyneConverter::Publish() {
    if (idx_ < all_stamps_.size()) {
        std::string st = std::to_string(all_stamps_[idx_].first);
        if (all_stamps_[idx_].second == "left") {
            std::string frame_file =
                dataset_dir_ + "/" + default_left_data_dir + "/" + st + ".bin";
            if (boost::filesystem::exists(frame_file)) {
                std::ifstream file;
                file.open(frame_file, std::ios::in | std::ios::binary);
                pcl::PointCloud<VelodynePointXYZIRT> pcl_cloud;
                while (!file.eof()) {
                    VelodynePointXYZIRT point;
                    file.read(reinterpret_cast<char *>(&point.x),
                              sizeof(float));
                    file.read(reinterpret_cast<char *>(&point.y),
                              sizeof(float));
                    file.read(reinterpret_cast<char *>(&point.z),
                              sizeof(float));
                    file.read(reinterpret_cast<char *>(&point.intensity),
                              sizeof(float));

                    float verticalAngle =
                        std::atan2(point.z, sqrt(point.x * point.x +
                                                 point.y * point.y)) *
                        180 / M_PI;
                    point.ring = (verticalAngle + 15.0) / 2.0;

                    pcl_cloud.points.push_back(point);
                }
                file.close();

                int cloud_size = pcl_cloud.points.size();
                float startOri =
                    -atan2(pcl_cloud.points[0].y, pcl_cloud.points[0].x);
                float endOri = -atan2(pcl_cloud.points[cloud_size - 1].y,
                                      pcl_cloud.points[cloud_size - 1].x) +
                               2 * M_PI;
                if (endOri - startOri > 3 * M_PI) {
                    endOri -= 2 * M_PI;
                } else if (endOri - startOri < M_PI) {
                    endOri += 2 * M_PI;
                }

                // range image projection
                bool halfPassed = false;
                for (int i = 0; i < cloud_size; ++i) {
                    float ori =
                        -atan2(pcl_cloud.points[i].y, pcl_cloud.points[i].x);
                    if (!halfPassed) {
                        if (ori < startOri - M_PI / 2) {
                            ori += 2 * M_PI;
                        } else if (ori > startOri + M_PI * 3 / 2) {
                            ori -= 2 * M_PI;
                        }
                        if (ori - startOri > M_PI) {
                            halfPassed = true;
                        }
                    } else {
                        ori += 2 * M_PI;
                        if (ori < endOri - M_PI * 3 / 2) {
                            ori += 2 * M_PI;
                        } else if (ori > endOri + M_PI / 2) {
                            ori -= 2 * M_PI;
                        }
                    }
                    float relTime = (ori - startOri) / (endOri - startOri);
                    pcl_cloud.points[i].time = scanPeriod * relTime;
                }

                sensor_msgs::PointCloud2 cloud;
                pcl::toROSMsg(pcl_cloud, cloud);
                cloud.header.stamp.fromNSec(all_stamps_[idx_].first);
                cloud.header.frame_id = "left_velodyne";

                pub_velodyne_left_.publish(cloud);
            }
        } else {
            std::string frame_file =
                dataset_dir_ + "/" + default_right_data_dir + "/" + st + ".bin";
            if (boost::filesystem::exists(frame_file)) {
                std::ifstream file;
                file.open(frame_file, std::ios::in | std::ios::binary);
                pcl::PointCloud<VelodynePointXYZIRT> pcl_cloud;
                while (!file.eof()) {
                    VelodynePointXYZIRT point;
                    file.read(reinterpret_cast<char *>(&point.x),
                              sizeof(float));
                    file.read(reinterpret_cast<char *>(&point.y),
                              sizeof(float));
                    file.read(reinterpret_cast<char *>(&point.z),
                              sizeof(float));
                    file.read(reinterpret_cast<char *>(&point.intensity),
                              sizeof(float));

                    float verticalAngle =
                        std::atan2(point.z, sqrt(point.x * point.x +
                                                 point.y * point.y)) *
                        180 / M_PI;
                    point.ring = (verticalAngle + 15.0) / 2.0;

                    pcl_cloud.points.push_back(point);
                }
                file.close();

                int cloud_size = pcl_cloud.points.size();
                float startOri =
                    -atan2(pcl_cloud.points[0].y, pcl_cloud.points[0].x);
                float endOri = -atan2(pcl_cloud.points[cloud_size - 1].y,
                                      pcl_cloud.points[cloud_size - 1].x) +
                               2 * M_PI;
                if (endOri - startOri > 3 * M_PI) {
                    endOri -= 2 * M_PI;
                } else if (endOri - startOri < M_PI) {
                    endOri += 2 * M_PI;
                }

                // range image projection
                bool halfPassed = false;
                for (int i = 0; i < cloud_size; ++i) {
                    float ori =
                        -atan2(pcl_cloud.points[i].y, pcl_cloud.points[i].x);
                    if (!halfPassed) {
                        if (ori < startOri - M_PI / 2) {
                            ori += 2 * M_PI;
                        } else if (ori > startOri + M_PI * 3 / 2) {
                            ori -= 2 * M_PI;
                        }
                        if (ori - startOri > M_PI) {
                            halfPassed = true;
                        }
                    } else {
                        ori += 2 * M_PI;
                        if (ori < endOri - M_PI * 3 / 2) {
                            ori += 2 * M_PI;
                        } else if (ori > endOri + M_PI / 2) {
                            ori -= 2 * M_PI;
                        }
                    }
                    float relTime = (ori - startOri) / (endOri - startOri);
                    pcl_cloud.points[i].time = scanPeriod * relTime;
                }

                sensor_msgs::PointCloud2 cloud;
                pcl::toROSMsg(pcl_cloud, cloud);
                cloud.header.stamp.fromNSec(all_stamps_[idx_].first);
                cloud.header.frame_id = "right_velodyne";

                pub_velodyne_right_.publish(cloud);
            }
        }

        return idx_ == all_stamps_.size() - 1 ? LONG_MAX
                                              : all_stamps_[++idx_].first;
    }

    return LONG_MAX;
}

void VelodyneConverter::Publish(int64_t &timestamp,
                                sensor_msgs::PointCloud2::Ptr &left_lidar,
                                sensor_msgs::PointCloud2::Ptr &right_lidar) {
    if (idx_ < all_stamps_.size()) {
        std::string st = std::to_string(all_stamps_[idx_].first);
        if (all_stamps_[idx_].second == "left") {
            std::string frame_file =
                dataset_dir_ + "/" + default_left_data_dir + "/" + st + ".bin";
            if (boost::filesystem::exists(frame_file)) {
                std::ifstream file;
                file.open(frame_file, std::ios::in | std::ios::binary);
                pcl::PointCloud<VelodynePointXYZIRT> pcl_cloud;
                while (!file.eof()) {
                    VelodynePointXYZIRT point;
                    file.read(reinterpret_cast<char *>(&point.x),
                              sizeof(float));
                    file.read(reinterpret_cast<char *>(&point.y),
                              sizeof(float));
                    file.read(reinterpret_cast<char *>(&point.z),
                              sizeof(float));
                    file.read(reinterpret_cast<char *>(&point.intensity),
                              sizeof(float));

                    float verticalAngle =
                        std::atan2(point.z, sqrt(point.x * point.x +
                                                 point.y * point.y)) *
                        180 / M_PI;
                    point.ring = (verticalAngle + 15.0) / 2.0;

                    pcl_cloud.points.push_back(point);
                }
                file.close();

                int cloud_size = pcl_cloud.points.size();
                float startOri =
                    -atan2(pcl_cloud.points[0].y, pcl_cloud.points[0].x);
                float endOri = -atan2(pcl_cloud.points[cloud_size - 1].y,
                                      pcl_cloud.points[cloud_size - 1].x) +
                               2 * M_PI;
                if (endOri - startOri > 3 * M_PI) {
                    endOri -= 2 * M_PI;
                } else if (endOri - startOri < M_PI) {
                    endOri += 2 * M_PI;
                }

                // range image projection
                bool halfPassed = false;
                for (int i = 0; i < cloud_size; ++i) {
                    float ori =
                        -atan2(pcl_cloud.points[i].y, pcl_cloud.points[i].x);
                    if (!halfPassed) {
                        if (ori < startOri - M_PI / 2) {
                            ori += 2 * M_PI;
                        } else if (ori > startOri + M_PI * 3 / 2) {
                            ori -= 2 * M_PI;
                        }
                        if (ori - startOri > M_PI) {
                            halfPassed = true;
                        }
                    } else {
                        ori += 2 * M_PI;
                        if (ori < endOri - M_PI * 3 / 2) {
                            ori += 2 * M_PI;
                        } else if (ori > endOri + M_PI / 2) {
                            ori -= 2 * M_PI;
                        }
                    }
                    float relTime = (ori - startOri) / (endOri - startOri);
                    pcl_cloud.points[i].time = scanPeriod * relTime;
                }

                left_lidar.reset(new sensor_msgs::PointCloud2());
                pcl::toROSMsg(pcl_cloud, *left_lidar);
                left_lidar->header.stamp.fromNSec(all_stamps_[idx_].first);
                left_lidar->header.frame_id = "left_velodyne";

                // pub_velodyne_left_.publish(cloud);
            }
        } else {
            std::string frame_file =
                dataset_dir_ + "/" + default_right_data_dir + "/" + st + ".bin";
            if (boost::filesystem::exists(frame_file)) {
                std::ifstream file;
                file.open(frame_file, std::ios::in | std::ios::binary);
                pcl::PointCloud<VelodynePointXYZIRT> pcl_cloud;
                while (!file.eof()) {
                    VelodynePointXYZIRT point;
                    file.read(reinterpret_cast<char *>(&point.x),
                              sizeof(float));
                    file.read(reinterpret_cast<char *>(&point.y),
                              sizeof(float));
                    file.read(reinterpret_cast<char *>(&point.z),
                              sizeof(float));
                    file.read(reinterpret_cast<char *>(&point.intensity),
                              sizeof(float));

                    float verticalAngle =
                        std::atan2(point.z, sqrt(point.x * point.x +
                                                 point.y * point.y)) *
                        180 / M_PI;
                    point.ring = (verticalAngle + 15.0) / 2.0;

                    pcl_cloud.points.push_back(point);
                }
                file.close();

                int cloud_size = pcl_cloud.points.size();
                float startOri =
                    -atan2(pcl_cloud.points[0].y, pcl_cloud.points[0].x);
                float endOri = -atan2(pcl_cloud.points[cloud_size - 1].y,
                                      pcl_cloud.points[cloud_size - 1].x) +
                               2 * M_PI;
                if (endOri - startOri > 3 * M_PI) {
                    endOri -= 2 * M_PI;
                } else if (endOri - startOri < M_PI) {
                    endOri += 2 * M_PI;
                }

                // range image projection
                bool halfPassed = false;
                for (int i = 0; i < cloud_size; ++i) {
                    float ori =
                        -atan2(pcl_cloud.points[i].y, pcl_cloud.points[i].x);
                    if (!halfPassed) {
                        if (ori < startOri - M_PI / 2) {
                            ori += 2 * M_PI;
                        } else if (ori > startOri + M_PI * 3 / 2) {
                            ori -= 2 * M_PI;
                        }
                        if (ori - startOri > M_PI) {
                            halfPassed = true;
                        }
                    } else {
                        ori += 2 * M_PI;
                        if (ori < endOri - M_PI * 3 / 2) {
                            ori += 2 * M_PI;
                        } else if (ori > endOri + M_PI / 2) {
                            ori -= 2 * M_PI;
                        }
                    }
                    float relTime = (ori - startOri) / (endOri - startOri);
                    pcl_cloud.points[i].time = scanPeriod * relTime;
                }

                right_lidar.reset(new sensor_msgs::PointCloud2());
                pcl::toROSMsg(pcl_cloud, *right_lidar);
                right_lidar->header.stamp.fromNSec(all_stamps_[idx_].first);
                right_lidar->header.frame_id = "right_velodyne";

                // pub_velodyne_right_.publish(cloud);
            }
        }

        timestamp =
            (idx_ == all_stamps_.size() - 1 ? LONG_MAX
                                            : all_stamps_[++idx_].first);
        return;
    }

    timestamp = LONG_MAX;
    return;
}

int VelodyneConverter::Convert() {
    CheckAndCreateSaveDir();

    boost::filesystem::path left_bag_file =
        boost::filesystem::path(save_dir_) / left_bag_name_;
    boost::filesystem::path right_bag_file =
        boost::filesystem::path(save_dir_) / right_bag_name_;

    const std::string left_stamp_file =
        dataset_dir_ + "/" + default_left_stamp_file;
    const std::string left_data_dir =
        dataset_dir_ + "/" + default_left_data_dir;
    const std::string right_stamp_file =
        dataset_dir_ + "/" + default_right_stamp_file;
    const std::string right_data_dir =
        dataset_dir_ + "/" + default_right_data_dir;

    // ROS_INFO("saving %s", left_bag_file.c_str());
    Convert(left_stamp_file, left_data_dir, left_bag_file.string(), left_topic_,
            "left_velodyne");
    // ROS_INFO("done saving %s", left_bag_file.c_str());
    // ROS_INFO("saving %s", right_bag_file.c_str());
    Convert(right_stamp_file, right_data_dir, right_bag_file.string(),
            right_topic_, "right_velodyne");
    // ROS_INFO("done saving %s", right_bag_file.c_str());

    return 0;
}

void VelodyneConverter::Convert(const std::string &stamp_file,
                                const std::string &data_dir,
                                const std::string &bag_file,
                                const std::string &topic,
                                const std::string &frame_id) {
    // rosbag::Bag bag(bag_file, rosbag::bagmode::Write);
    // bag.setChunkThreshold(768*1024);
    // bag.setCompression(rosbag::compression::BZ2);

    FILE *fp = fopen(stamp_file.c_str(), "r");
    int64_t stamp;
    std::vector<int64_t> all_stamps;
    while (fscanf(fp, "%ld\n", &stamp) == 1) {
        all_stamps.push_back(stamp);
    }
    fclose(fp);

    size_t total = all_stamps.size();
    for (size_t i = 0; i < all_stamps.size(); ++i) {
        std::string st = std::to_string(all_stamps[i]);
        std::string frame_file = data_dir + "/" + st + ".bin";
        // ROS_INFO("converting %s\n", frame_file.c_str());
        if (!boost::filesystem::exists(frame_file)) {
            // ROS_WARN("%s not exist\n", frame_file.c_str());
            continue;
        }
        std::ifstream file;
        file.open(frame_file, std::ios::in | std::ios::binary);
        pcl::PointCloud<pcl::PointXYZI> pcl_cloud;
        while (!file.eof()) {
            pcl::PointXYZI point;
            file.read(reinterpret_cast<char *>(&point.x), sizeof(float));
            file.read(reinterpret_cast<char *>(&point.y), sizeof(float));
            file.read(reinterpret_cast<char *>(&point.z), sizeof(float));
            file.read(reinterpret_cast<char *>(&point.intensity),
                      sizeof(float));
            pcl_cloud.points.push_back(point);
        }
        file.close();
        sensor_msgs::PointCloud2 cloud;
        pcl::toROSMsg(pcl_cloud, cloud);
        cloud.header.stamp.fromNSec(all_stamps[i]);
        cloud.header.frame_id = frame_id;

        if (frame_id == "left_velodyne") {
            pub_velodyne_left_.publish(cloud);
        } else {
            pub_velodyne_right_.publish(cloud);
        }

        // bag.write(topic, cloud.header.stamp, cloud);
        // ROS_INFO("bag write %s, %u points\n", topic.c_str(), cloud.height *
        // cloud.width); ROS_INFO("done converting %s\n", frame_file.c_str());
        // ROS_INFO("total %lu, already convert %lu, remain %lu\n", total, i +
        // 1, total - i - 1);
    }
    // bag.close();
}

}  // namespace kaist_publisher